/*
 * main.cpp
 *
 *      Author: Christian
 *       Letztes Update: 11.01.14 Christian | fähler rausmachen
 *
 */

#ifndef F_CPU
#define F_CPU 16000000UL
#endif

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <avr/eeprom.h>

#include "MPU_6050.h"
#include "Feedback.h"
#include "UART.h"
#include "Empf.h"
#include "Scheduler.h"
#include "ppmGen.h"
//#include "regler.h"
#include "config.h"
#include "filter.h"
#include "control.h"

void config_mode();
void normal_mode();
void store_config();
void update_eeprom_adr();
int main() {

		normal_mode();

	return 0;
}

void normal_mode() {
	// Initialisierung

	//uint8_t bla = 0;

	feedback_init();
//	regler_init();
	control_init();
	mpu_init();

	if (!mpu_bias_ok()) {
		system_reset();
	}
	UARTInit();
	empfaenger_init();
	ppmGenInit();
	filter_init();
	scheduler_start();

	sei();

	// Programmschleife
	while (1) {

		if (status & (1 << NEWSENS)) {
			// Laufzeitmessung
			uint8_t t = TCNT0;
			filter_update();
			t = TCNT0 - t;
			send_buffer[18] = t;

			//calcMotorValues(empf_wert[1], (float) (empf_wert[2] - 50),
			//		(float) (-1 * (empf_wert[0] - 50)),
			//		40 * (empf_wert[3] - 50));
			//calcMotorValues( empf_wert[1], ( empf_wert[2] - 50 ),( empf_wert[2] - 50 ), (empf_wert[2] - 50 ));

			control(empf_wert[1], empf_wert[2] - 50, empf_wert[0] - 50,
				empf_wert[3] - 50);



			ppmSendSignal(motor_values[0], motor_values[1], motor_values[2],
					motor_values[3]);

			status &= ~(1 << NEWSENS);

		}

		// Debugging aufgaben

		if (receive_buffer[0] == SEND_DEBUG_MESSAGE) {
			reset_receive_buffer();
			sendDebugMessage();

		} else if (receive_buffer[0] == REGLERWERTE_UPDATE) {
			reset_receive_buffer();
			//reglerwerte_update();
			control_pid_update();
		} else if (receive_buffer[0] == KALMAN_FILTER_UPDATE) {
			reset_receive_buffer();
			//kalman_setup_update();
			filter_coefficent_update();
		} else if (receive_buffer[0] == SYSTEM_RESET) {
			reset_receive_buffer();
			system_pause();
			system_reset();
		} else if (receive_buffer[0] == UPDATE_ALL_CONFIG) {

			store_config();

		}else if (receive_buffer[0] == UPDATE_EEPROM_BYTE) {

			update_eeprom_adr();

		}
		else if ((uint8_t) receive_buffer[0] == RESTART_IN_CONFIG_MODE) {
			reset_receive_buffer();
			system_pause();
			eeprom_update_byte((uint8_t *) EEPROM_ADR_MODE, 0x01);
			system_reset();
		}

	}
}

void config_mode() {

	eeprom_update_byte((uint8_t *) EEPROM_ADR_MODE, 0x00);
	system_reset();
}

void store_config() {
	// TODO: alle pid werte

	while (received_number < 11)
		;

	system_pause();

	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_XY_ANGLE, receive_buffer[1]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_XY_ANGLE, receive_buffer[2]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_XY_ANGLE, receive_buffer[3]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_Z_RATE, receive_buffer[4]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_Z_RATE, receive_buffer[5]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_Z_RATE, receive_buffer[6]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_Q_ANGLE, receive_buffer[7]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_Q_GYRO, receive_buffer[8]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_R_ANGLE, receive_buffer[9]);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_LOW_PASS, receive_buffer[10]);

	reset_receive_buffer();

	system_reset();
}

void update_eeprom_adr() {
	while (received_number < 3);


	eeprom_update_byte((uint8_t *) receive_buffer[1] ,receive_buffer[2]);
	reset_receive_buffer();

}
